#include "foc.h"

#define sqrt3_2  0.8660254f  //����3/2
#define _2PI 6.28318f 			// 2PI
#define polePairs 7			// ����ļ�����
#define sqrt3			1.732f				//����3

float zero=0.0f;//��Ƕȵ�
FOC_DEF foc;






float limit(float in_vo,float low,float high)
{
	if(in_vo>=high)
		in_vo=high;
	else if(in_vo<=low)
		in_vo=low;
	else
		in_vo=in_vo;
	return in_vo;
}

void setPhaseVoltage(float Uq, float Ud, float angle_)
{
	float pwm_a=0,pwm_b=0,pwm_c=0;
	float Ualpha=0.0f,Ubate=0.0f;
	float Ua=0.0f,Ub=0.0f,Uc=0.0f;
	//�����޷�
	Uq = limit(Uq,-6,6);
	

	
	//park��任
	Ualpha = -Uq * sin(angle_) + Ud * cos(angle_);
	Ubate  =  Uq * cos(angle_) + Ud * sin(angle_);

	//clarke��任
	Ua = Ualpha + 6;
	Ub = (sqrt3 * Ubate - Ualpha)/2 + 6;
	Uc = (-sqrt3 * Ubate - Ualpha)/2 + 6;
	
	Ua = limit(Ua,0.0f,12);
	Ub = limit(Ub,0.0f,12);
	Uc = limit(Uc,0.0f,12);
	
	//PWM�޷�
	pwm_a = limit(Ua / 12 , 0.0f , 1.0f);
	pwm_b = limit(Ub / 12 , 0.0f , 1.0f);
	pwm_c = limit(Uc / 12 , 0.0f , 1.0f);
	
	set_pwm(pwm_a*5000,pwm_b*5000,pwm_c*5000);
}














void svpwm_1(float U_beta,float U_alpha)
{
	float U1,U2,U3;
	int sector=0;
	float Ts,T1,T2,T3,T4,T5,T6,T7,Ta,Tb,Tc,sum,k;
	
	//����U1��U2��U3
	U1=U_beta;
	U2=-sqrt3_2*U_alpha-0.5f*U_beta;
	U3=sqrt3_2*U_alpha-0.5f*U_beta;
	//�����ж�  N=A+2B+4C
	if(U1>0){sector=sector+1;}
	if(U2>0){sector=sector+2;}
	if(U3>0){sector=sector+4;}
	Ts=1.0f;//svpwm��������
	//�����ѹ����ʱ��
	switch (sector)
	{
		case 1:
			//����1
			T2=-U3;
			T6=-U2;
			sum=T2+T6;
			if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T2=k*T2;
				T6=k*T6;
			}
			T7=(Ts-T2-T6)/2;
			Ta=T6+T7;
			Tb=T2+T6+T7;
			Tc=T7;
			break;
		case 2 :
			T1=-U1;
			T3=-U3;
			sum=T1+T3;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T1=k*T1;
				T3=k*T3;
			}
			T7=(Ts-T1-T3)/2;
			Ta=T7;
			Tb=T3+T7;
			Tc=T7+T1+T3;
			break;
		case 3 :
			T2=U1;
			T3=U2;
			sum=T2+T3;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T2=k*T2;
				T3=k*T3;
			}
			T7=(Ts-T2-T3)/2;
			Ta=T7;
			Tb=T3+T7+T2;
			Tc=T7+T3;
			break;
		case 4 :
			T4=-U2;
			T5=-U1;
			sum=T4+T5;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T4=k*T4;
				T5=k*T5;
			}
			T7=(Ts-T5-T4)/2;
			Ta=T7+T4+T5;
			Tb=T7;
			Tc=T7+T5;
			break;
		case 5 :
			T4=U3;
			T6=U1;
			sum=T4+T6;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T4=k*T4;
				T6=k*T6;
			}
			T7=(Ts-T4-T6)/2;
			Ta=T7+T4+T6;
			Tb=T6+T7;
			Tc=T7;
			break;
		case 6 :
			T1=U2;
			T5=U3;
			sum=T1+T5;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T1=k*T1;
				T5=k*T5;
			}
			T7=(Ts-T1-T5)/2;
			Ta=T7+T5;
			Tb=T7;
			Tc=T7+T1+T5;
			break;
	
		default :
			break;
	}
	set_pwm(Ta*5000,Tb*5000,Tc*5000);
	vofa_FireWater_output(Ta*2000,Tb*2000,Tc*2000);
}



void svpwm(float Iq,float Id,float angle)
{
	float U_alpha,U_beta;
	float U1,U2,U3;
	int sector=0;
	float Ts,T1,T2,T3,T4,T5,T6,T7,Ta,Tb,Tc,sum,k;
	//park��任
	U_alpha=Id*cos(angle)-Iq*sin(angle);
	U_beta=Id*sin(angle)+Iq*cos(angle);
	//����U1��U2��U3
	U1=U_beta;
	U2=-sqrt3_2*U_alpha-0.5f*U_beta;
	U3=sqrt3_2*U_alpha-0.5f*U_beta;
	//�����ж�  N=A+2B+4C
	if(U1>0){sector=sector+1;}
	if(U2>0){sector=sector+2;}
	if(U3>0){sector=sector+4;}
	Ts=1.0f;//svpwm��������
	//�����ѹ����ʱ��
	switch (sector)
	{
		case 1:
			//����1
			T2=-U3;
			T6=-U2;
			sum=T2+T6;
			if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T2=k*T2;
				T6=k*T6;
			}
			T7=(Ts-T2-T6)/2;
			Ta=T6+T7;
			Tb=T2+T6+T7;
			Tc=T7;
			break;
		case 2 :
			T1=-U1;
			T3=-U3;
			sum=T1+T3;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T1=k*T1;
				T3=k*T3;
			}
			T7=(Ts-T1-T3)/2;
			Ta=T7;
			Tb=T3+T7;
			Tc=T7+T1+T3;
			break;
		case 3 :
			T2=U1;
			T3=U2;
			sum=T2+T3;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T2=k*T2;
				T3=k*T3;
			}
			T7=(Ts-T2-T3)/2;
			Ta=T7;
			Tb=T3+T7+T2;
			Tc=T7+T3;
			break;
		case 4 :
			T4=-U2;
			T5=-U1;
			sum=T4+T5;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T4=k*T4;
				T5=k*T5;
			}
			T7=(Ts-T5-T4)/2;
			Ta=T7+T4+T5;
			Tb=T7;
			Tc=T7+T5;
			break;
		case 5 :
			T4=U3;
			T6=U1;
			sum=T4+T6;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T4=k*T4;
				T6=k*T6;
			}
			T7=(Ts-T4-T6)/2;
			Ta=T7+T4+T6;
			Tb=T6+T7;
			Tc=T7;
			break;
		case 6 :
			T1=U2;
			T5=U3;
			sum=T1+T5;
		    if(sum > Ts)
			{
				k=Ts/sum;//��������ϵ��
				T1=k*T1;
				T5=k*T5;
			}
			T7=(Ts-T1-T5)/2;
			Ta=T7+T5;
			Tb=T7;
			Tc=T7+T1+T5;
			break;
	
		default :
			break;
	}
	set_pwm(Ta*5000,Tb*5000,Tc*5000);
//	vofa_FireWater_output(Ta*2000,Tb*2000,Tc*2000);
//	vofa_FireWater_output(U3,U1,U2);
}

// �ѽǶ�ֵ��һ���� [0, 2pi]
float Angle_limit(float angle)
{
    float a = fmod(angle, _2PI); // fmod()�������ڸ�������ȡ������
    return a >= 0.0f ? a : (a + _2PI);
}

// ��Ƕ� = ��е�Ƕ� * ������
float _electricalAngle(float shaft_angle)
{
    return Angle_limit(shaft_angle * polePairs - zero);
}

//float angle_init(void)//����Ƕ�
//{
//	
//	HAL_Delay(100);
//	zero = _electricalAngle(i2c_AS5600_get_angle());
//	float zero_angle = _electricalAngle(i2c_AS5600_get_angle());
//	
//	
//}
//void loop_motor(float angle_zero,float IQ)//����
//{
//	float new_angle1,angle1;
//	new_angle1=i2c_AS5600_get_angle();
//	  if(new_angle1>=angle_zero)
//	  {
//		new_angle1=new_angle1-angle_zero;
//	  }
//	  else
//	  {
//		new_angle1=_2PI-angle_zero+new_angle1;
//	  }
//	  new_angle1=new_angle1*7;
//	  set_pwm(IQ,0,new_angle1);
//}


float motor_init(float Ud)
{
	float theta=0.0f;
	svpwm(0,Ud,0);
	HAL_Delay(1000);
	theta=i2c_AS5600_get_angle();
	
	return theta;
}

float get_angle(float angle_zero)//
{
	float ele_angle=0,new_angle=0;
	new_angle=-foc.the;
	if(new_angle>angle_zero)
	{
		new_angle=new_angle-angle_zero;
	}
	else
	{
		new_angle=new_angle-angle_zero+_2PI;
	}
	//new_angle=new_angle-angle_zero;
	
	ele_angle=new_angle*7;
//	vofa_FireWater_output(ele_angle,new_angle,0);
	ele_angle=fmod(ele_angle,_2PI);
	return ele_angle;
}

float get_angle_2(float angle_zero)//
{
	float ele_angle=0,new_angle=0;
	new_angle=foc.the;
	if(new_angle>angle_zero)
	{
		new_angle=new_angle-angle_zero;
	}
	else
	{
		new_angle=new_angle-angle_zero+_2PI;
	}
//	new_angle=new_angle-angle_zero;
	
	ele_angle=new_angle*7;
//	vofa_FireWater_output(ele_angle,new_angle,0);
	ele_angle=fmod(ele_angle,2 * PI);
	return ele_angle;
}

float get_angle_1(float angle_zero,float new_angle)//
{
	float ele_angle=0;
	
	if(new_angle>angle_zero)
	{
		new_angle=new_angle-angle_zero;
	}
	else
	{
		new_angle=new_angle-angle_zero+_2PI;
	}
	ele_angle=new_angle*7;
	//vofa_FireWater_output(ele_angle,new_angle,0);
	ele_angle=fmod(ele_angle,2 * PI);
	return ele_angle;
}

void loop_motor(float UQ,float angle_zero)//svpwm
{
	float angle=0.0f;
	angle=get_angle(angle_zero);
	svpwm(UQ,0,angle);
}
void loop_motor_test(float UQ,float angle_zero)
{
	float angle=0.0f;
	angle=get_angle_2(angle_zero);
	foc.the=angle;
	setPhaseVoltage(UQ,0,angle);
}

void Set_Angle(float Angle)
{
//	float angle,e_angle,Angle_Out,fla=0.0f;
//	//�Ƕȶ�ȡ
//	angle = i2c_AS5600_get_angle();
//	//angle=angle_get_all(angle);
//	//�Ƕȿ���
//	fla=(Angle - 1*angle)*180/PI;
////	Angle_Out = Angle_Control(fla,Angle);
//	//
//	Angle_Out=limit(Angle_Out,-6,6);
//	e_angle=get_angle_1(3.5,angle);
//	setPhaseVoltage(Angle_Out,0,e_angle);
	//�Ƕȴ�ӡ
	//printf("%.2f,%.2f\n",Angle_Out,fla);
}






#define PWM_Period 100
#define _PI 3.14159265359
#define _PI_2 1.57079632679
#define _PI_3 1.0471975512

#define _3PI_2 4.71238898038
#define _PI_6 0.52359877559
#define _SQRT3 1.73205080757

/******************************************************************************/
float voltage_power_supply;
float sensor_offset=0;
float zero_electric_angle=0;
/******************************************************************************/
// normalizing radian angle to [0,2PI]
float _normalizeAngle(float angle)
{
  float a = fmod(angle, _2PI);                     //fmod()�Ը�����ȡģ
  return a >= 0 ? a : (a + _2PI);
}
/******************************************************************************/
//FOC���ĺ���������Ud��Uq�͵�Ƕȣ����PWM
void setPhaseVoltage_1(float Uq, float Ud, float angle_el)
{
	float Uref;
	uint32_t sector;
	float T0,T1,T2;
	float Ta,Tb,Tc;
	float U_alpha,U_beta;
	
	angle_el =_normalizeAngle(angle_el);                    //��Ƕȱ�׼���ڡ�0,2pi��

	U_alpha=Ud*cos(angle_el)-Uq*sin(angle_el);            //��park�任
	U_beta=Ud*sin(angle_el)+Uq*cos(angle_el);
	
	Uref=_sqrt(U_alpha*U_alpha + U_beta*U_beta) / voltage_power_supply;
	
	if(Uref> 0.577)Uref= 0.577;                     			//�����ε�����Բ(SVPWM���ʧ����ת��ѹʸ����ֵ)����3/3
	if(Uref<-0.577)Uref=-0.577; 
	
	if(Uq>0)
	  angle_el =_normalizeAngle(angle_el+_PI_2);            //��90�Ⱥ��ǲο���ѹʸ����λ��
	else
		angle_el =_normalizeAngle(angle_el-_PI_2);
	sector = (angle_el / _PI_3) + 1;                			//���ݽǶ��жϲο���ѹ��������                        

	T1 = _SQRT3*sin(sector*_PI_3 - angle_el) * Uref;           //�����������ڵ�ѹʸ������ʱ��
	T2 = _SQRT3*sin(angle_el - (sector-1.0)*_PI_3) * Uref;
	T0 = 1 - T1 - T2;                                          //��ʸ������ʱ��
	
	// calculate the duty cycles(times)
	switch(sector) 
	{
		case 1:
			Ta = T1 + T2 + T0/2;
			Tb = T2 + T0/2;
			Tc = T0/2;
			break;
		case 2:
			Ta = T1 +  T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T0/2;
			break;
		case 3:
			Ta = T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T2 + T0/2;
			break;
		case 4:
			Ta = T0/2;
			Tb = T1+ T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 5:
			Ta = T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 6:
			Ta = T1 + T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T0/2;
			break;
		default:  // possible error state
			Ta = 0;
			Tb = 0;
			Tc = 0;
	}
		set_pwm(Ta*5000,Tb*5000,Tc*5000);
	vofa_FireWater_output(Ta*2000,Tb*2000,Tc*2000);

}
/**********************************************************************************************************
Clarke�任 ����������������alpha��bate����
I�� = Ia
I�� = (Ia + 2Ib) / sqrt(3)
**********************************************************************************************************/
void clarkeTransform(ABC_Def *abc, AlphaBeta_Def *alphaBeta)
{
    alphaBeta->alpha = abc->Ia;
    alphaBeta->beta = (abc->Ia + 2 * abc->Ib) * 0.57735027f;
}

/**********************************************************************************************************
Park�任�������Ƕȡ�Ialpha��Ibeta������Park�任�õ�Iq��Id
Id = I�� �� cos�� + I�� �� sin��
Iq = I�� �� sin�� + I�� �� cos��
**********************************************************************************************************/
void parkTransform(AlphaBeta_Def *alphaBeta, float angle, DQ_Def *dq)
{
   
    dq->q = cos(angle) * alphaBeta->alpha + sin(angle) * alphaBeta->beta;
    dq->d = sin(angle) * alphaBeta->alpha - cos(angle) * alphaBeta->beta;
}

void speed_set(float vel_sp)
{
	float angle1_speed=0,angle_speed=0,vel_LPF=0,Uq_set=0,angle_el=0;
	
	angle1_speed=i2c_AS5600_get_angle();
	angle_speed=get_speed();
//	vel_LPF=LPF_velocity(angle_speed);
	Uq_set = PID_velocity(vel_sp-vel_LPF);
	angle_el=get_angle_1(3.5,angle1_speed);
	svpwm(Uq_set,0,angle_el);
	vofa_FireWater_output(vel_LPF,Uq_set,30);
}




